Lab 5: Linear PID control and Linear interpolation

1. Position Control setup

With the BLE command interface and logging framework already set up in the pre-lab, I then implemented the wall-distance controller that allows the robot to approach and maintain a desired distance from a wall. The controller was implemented in control.hpp and uses feedback from the front Time-of-Flight sensor to continuously update the motor commands. For all of my experiments, I used a target distance of approximately 304 mm from the wall.

During each control loop iteration, the robot reads the ToF measurement, computes the distance error, calculates the control output, applies limits, sends motor commands, and logs controller data.

Distance Measurement and Error Calculation

For the controller I only used the front ToF sensor and it was configured in long distance mode with a timing budget of about 20 ms. In my hardware setup this sensor is the XSHUT-controlled sensor, which is stored as g_tof2. I created a helper function that returns the most recent valid distance measurement:

static inline float control_get_raw_distance_mm() {
  if (g_tof2_last.valid) return (float)g_tof2_last.d_mm;
  return 0.0f;
}

The controller then computes the wall-distance error using equation:

\[ e(t) = \text{setpoint} - \text{distance} \]

const float error = g_pid.setpoint_mm - dist_mm;

If the robot is farther than the desired distance, the error becomes positive and the robot should drive forward. If the robot is too close to the wall, the error becomes negative and the robot should move backward.

PID Controller Implementation

The robot motion is controlled using a PID controller equation:

\[ u(t) = K_p e(t) + K_i \int e(t)\,dt + K_d \frac{de(t)}{dt} \]

In my implementation the PID controller is computed in discrete time using the following code:

const float error = g_pid.setpoint_mm - dist_mm;
        float dt = (now_ms - g_pid.prev_t_ms) * 0.001f;
        if (dt <= 0.0f) dt = 0.001f;
        // integral term
        g_pid.integral += error * dt;
        g_pid.integral = clamp(g_pid.integral, g_pid.integral_min, g_pid.integral_max);
        // derivative term
        const float derivative = (error - g_pid.prev_error) / dt;
        // individual PID terms
        const float p_term = g_pid.kp * error;
        const float i_term = g_pid.ki * g_pid.integral;
        const float d_term = g_pid.kd * derivative;
        // final controller output
        float u = p_term + i_term + d_term;

The time step dt is computed from the system clock. The integral term accumulates the error over time and is clamped to prevent integral windup. The derivative term estimates the rate of change of the error between successive control updates.

Output Limiting and Motor Command

During my first experiments the robot approached the wall too quickly and often overshot the desired distance. To improve this behavior, I implemented a distance-dependent output limiter so that the maximum control signal decreases as the robot gets closer to the target.

static inline float control_get_dynamic_limit(float error_abs_mm) {
          if (error_abs_mm > 400.0f) return 0.25f;
          if (error_abs_mm > 200.0f) return 0.15f;
          if (error_abs_mm > 80.0f)  return 0.08f;
          return 0.04f;
        }

The controller output is then limited and applied to the motors:

float u_limit = control_get_dynamic_limit(fabs(error));
          u = clamp(u, -u_limit, u_limit);
          // stop band near the setpoint
          if (fabs(error) < 10.0f) {
            motors_stop();
            g_pid.prev_error = error;
            g_pid.prev_t_ms = now_ms;
            return;
          }
          // send command to motors
          if (g_motors_enabled) {
            motors_set_normalized(-u);
          } else {
            motors_stop();
          }

This section of the code performs three tasks. First, it limits the maximum control signal so the robot slows down as it approaches the wall. Second, it introduces a small stop band around the setpoint to prevent constant oscillations near the target distance. Finally, it converts the normalized control signal into motor commands.

I used -u in the motor command because of the motor orientation on my robot, so a positive control output drives the robot toward the wall.

2. PID

P

I first tested the controller using only proportional control with gains K_p = 0.001, K_i = 0, and K_d = 0. As shown in the ToF vs time plot, the robot approaches the wall quickly and slows down as the error decreases. However, the robot overshoots and settles too close to the wall. The motor input plot shows the controller applying a small corrective command after the overshoot, but the output becomes too small to move the robot back to the setpoint.

PI

To reduce the steady-state error observed with proportional control, I added an integral term and tested a PI controller with gains K_p = 0.001, K_i = 0.00015, and K_d = 0. As shown in the plots, the robot approaches the wall and settles closer to the target distance compared to the proportional-only controller. The integral term accumulates the error over time, which increases the control signal and helps overcome motor deadband and friction.

PID

Finally, I tested a full PID controller with gains K_p = 0.001, K_i = 0.00015, and K_d = 0.00015. As shown in the plots, the robot approaches the wall and settles closest to the desired distance compared with the previous controllers. The derivative term helps slow the robot as it nears the setpoint, which reduces overshoot and improves the final accuracy. However, the PID terms plot shows that the derivative term produces large spikes due to noise in the ToF measurements, which also appears as fluctuations in the motor input. From this observation, a low-pass filter on the derivative term would likely improve stability by reducing the effect of sensor noise.

I also tested the controller under external perturbations by pushing the robot both away from the wall and toward the wall during operation. As shown in the video below, the robot was able to recover and return to the setpoint.

3. Extrapolation

At first, the PID controller was limited by the update rate of the ToF sensor because the controller could only react when a new distance measurement became available. To improve this, I decoupled the control loop from the sensor update rate so that the PID controller runs every loop iteration, even if the ToF sensor has not produced a new reading yet.

In the main loop, the ToF update and the control update are handled separately. The controller runs whenever control is enabled, while the ToF sensor only updates when new data is available.

void loop() {
          imu_update();
          if (g_tof_running) {
            tof_update();
          }
          ble_update();
          if (g_control_enabled) {
            control_update();
          }
        }

I measured the approximate execution rates of both processes. The control loop runs at about 127.04 Hz, while the ToF sensor produces new measurements at about 9.98 Hz. This means the controller runs much faster than the sensor, so using only the last ToF reading can make the distance estimate lag behind the robot’s real motion.

To address this issue, I implemented a simple linear extrapolation algorithm to estimate the robot’s current distance between ToF updates. Every time a new ToF reading is received, I store the last two measurements along with their timestamps. These values are used to estimate the slope of the distance change:

\[ m = \frac{d_2 - d_1}{t_2 - t_1} \]

Then, if the controller runs before a new sensor measurement arrives, the current distance is estimated using the elapsed time since the last sample:

\[ d_{est} = d_2 + m (t - t_2) \]

static inline float control_get_estimated_distance_mm() {
  if (!g_tof2_last.valid) return 0.0f;
  if (!g_tof2_prev.valid) return (float)g_tof2_last.d_mm;

  float dt_hist = (g_tof2_last.t_ms - g_tof2_prev.t_ms) * 0.001f;
  if (dt_hist <= 0.0f) return (float)g_tof2_last.d_mm;

  float slope = ((float)g_tof2_last.d_mm - (float)g_tof2_prev.d_mm) / dt_hist;
  float dt_now = (millis() - g_tof2_last.t_ms) * 0.001f;
  return (float)g_tof2_last.d_mm + slope * dt_now;
}

In my controller implementation, both the raw distance and the extrapolated distance are available, but the extrapolated value is used for the control calculation because it better represents the robot's current position between ToF updates.

const float raw_dist_mm = control_get_raw_distance_mm();
          const float est_dist_mm = control_get_estimated_distance_mm();
          
          const float error = g_pid.setpoint_mm - est_dist_mm;

This allows the PID controller to keep updating even when the ToF sensor has not returned a new measurement yet. As a result, the control loop can operate at the full controller rate while still incorporating new sensor readings whenever they become available.

The figure below compares the raw ToF measurements with the extrapolated distance estimate used by the controller.

4. wind-up protection

The PID controller implemented earlier already included an integral clamp to prevent wind-up. To demonstrate the importance of this protection, I compared the controller behavior using two different integral bounds.

In the first test, a loose bound (±2000) was used, which allows the integral term to accumulate a large amount of error during the robot’s approach to the wall. In the second test, the bound was tightened to ±200, which actively limits the integral accumulation.

From the plot above we can see that the integral term grows larger during the approach with the loose clamp, causing the robot to overshoot significantly and drive very close to the wall before recovering. And with the tighter clamp where the accumulated error is limited, which reduces overshoot and allows the robot to settle closer to the set point.